Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Writing a Force Controller (Python)

In this tutorial, we will use the force/impedance realtime controller from the ee_cart_imped_control package to execute a trajectory that uses force control on the PR2 arm.

You should first navigate to the directory you created when you wrote the stiffness controller:

roscd ee_cart_imped_tutorial

The Code

Within this package, create the file scripts/force_control.py and paste the following code into it

   1 #!/usr/bin/env python
   2 import roslib; roslib.load_manifest('ee_cart_imped_tutorial')
   3 import rospy
   4 import ee_cart_imped_action
   5 
   6 def main():
   7     control = ee_cart_imped_action.EECartImpedClient('right_arm')
   8     control.addTrajectoryPoint(0.5, 0, 0, 0, 0, 0, 1,
   9                                1000, 1000, 1000, 30, 30, 30,
  10                                False, False, False, False, False,
  11                                False, 4, '/torso_lift_link');
  12     control.addTrajectoryPoint(0.75, 0, 0, 0, 0, 0, 1,
  13                                10, 1000, 1000, 30, 30, 30,
  14                                True, False, False, False, False,
  15                                False, 6, '/torso_lift_link');
  16     control.sendGoal()
  17 
  18 if __name__ == '__main__':
  19     rospy.init_node('force_control_test')
  20     main()

Make the file executable:

roscd ee_cart_imped_tutorial/scripts
chmod +x force_control.py

Examining the Code

This code is very similar to the code you used in when doing stiffness control. The only change is in the trajectory point we add on line 12:

  12     control.addTrajectoryPoint(0.75, 0, 0, 0, 0, 0, 1,
  13                                10, 1000, 1000, 30, 30, 30,
  14                                True, False, False, False, False,
  15                                False, 6, '/torso_lift_link');

Recall that the arguments to EECartImpedClient.addTrajectoryPoint are (goal, x, y, z, ox, oy, oz, ow, fx, fy, fz, tx, ty, tz, isfx, isfy, isfz, istx, isty, istz, time, frame_id). In the previous tutorial, we gave almost the exact same point. However, we have changed fx from 50 to 10 and isfx from False to True. Therefore, rather than having a small stiffness in the x direction as previously, we are now exerting a constant force of 10 N. Before, the gripper was attempting to maintain a position in x, although only weakly. Now, the gripper will exert a constant force in that direction without trying to achieve any point at all. The result will be, if there is nothing in the way, that the arm will slowly extend to its full range.

Running the Code

If you have not already, you must first download the ee_cart_imped stack and make it:

rosmake ee_cart_imped

If this is the first time you have made the stack, you must wait for the build to complete and then re-start the robot.

Now start the controller and action server. If you had already built the ee_cart_imped_control package before starting the robot or simulator, there is no need to re-start them.

And you are ready to run the code

rosrun ee_cart_imped_tutorial force_control.py

The PR2 arm should move to the center of the PR2 body and then extend away from the PR2 until it is completely extended. If you are working with the real PR2, you can put your hand in front of the gripper and feel the pressure it is exerting. When you remove your hand the arm should continue to extend:

Recall that the last point on the trajectory is held so the robot will continue to exert a force in the x direction until given a new trajectory.